<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>Aria: gpsRobotTaskExample.cpp</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
  $(window).load(resizeHeight);
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Aria
   &#160;<span id="projectnumber">2.9.1</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="pages.html"><span>Related&#160;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li><a href="files.html"><span>Files</span></a></li>
      <li><a href="examples.html"><span>Examples</span></a></li>
    </ul>
  </div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('gpsRobotTaskExample_8cpp-example.html','');});
</script>
<div id="doc-content">
<div class="header">
  <div class="headertitle">
<div class="title">gpsRobotTaskExample.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<p>Connects to both robot and GPS, allows teleoperation, and prints robot position and GPS data.</p>
<div class="fragment"><div class="line"></div><div class="line"></div><div class="line"><span class="preprocessor">#include &quot;Aria.h&quot;</span></div><div class="line"><span class="preprocessor">#include &quot;ArGPS.h&quot;</span></div><div class="line"><span class="preprocessor">#include &quot;ArGPSConnector.h&quot;</span></div><div class="line"></div><div class="line"><span class="preprocessor">#include &lt;assert.h&gt;</span></div><div class="line"></div><div class="line"></div><div class="line"><span class="comment">/*  </span></div><div class="line"><span class="comment"> *  This class encapsulates an ArRobot sensor interpretation task that prints the</span></div><div class="line"><span class="comment"> *  last set of GPS data along with a local timestamp and current robot pose to </span></div><div class="line"><span class="comment"> *  standard output.</span></div><div class="line"><span class="comment"> *</span></div><div class="line"><span class="comment"> *  This class also contains a mutex, which it locks during the task (while</span></div><div class="line"><span class="comment"> *  accessing the ArGPS object).  If another thread is also accessing the GPS,</span></div><div class="line"><span class="comment"> *  you can lock this mutex.</span></div><div class="line"><span class="comment"> */</span></div><div class="line"><span class="keyword">class </span>GPSLogTask {</div><div class="line"></div><div class="line"><span class="keyword">public</span>:</div><div class="line">  GPSLogTask(<a name="_a0"></a><a class="code" href="classArRobot.html">ArRobot</a> *robot, <a name="_a1"></a><a class="code" href="classArGPS.html">ArGPS</a> *gps, <a name="_a2"></a><a class="code" href="classArJoyHandler.html">ArJoyHandler</a> *joy = NULL) :</div><div class="line">      myRobot(robot), </div><div class="line">      myGPS(gps),</div><div class="line">      myTaskFunctor(<span class="keyword">this</span>, &amp;GPSLogTask::doTask),</div><div class="line">      myJoyHandler(joy),</div><div class="line">      myButtonDown(<span class="keyword">false</span>)</div><div class="line">  {</div><div class="line">    myRobot-&gt;addSensorInterpTask(<span class="stringliteral">&quot;GPS&quot;</span>, <a name="a3"></a><a class="code" href="classArListPos.html#adb42d52c70a646c5698c5513cd25bbd0a3c55111fa6c7f63c41fc0b387f5b57d9">ArListPos::LAST</a>, &amp;myTaskFunctor);   </div><div class="line">    puts(<span class="stringliteral">&quot;RobotX\tRobotY\tRobotTh\tRobotVel\tRobotRotVel\tRobotLatVel\tLatitude\tLongitude\tAltitude\tSpeed\tGPSTimeSec\tGPSTimeMSec\tFixType\tNumSats\tPDOP\tHDOP\tVDOP\tGPSDataReceived&quot;</span>);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">void</span> lock() { myMutex.lock(); }</div><div class="line">  <span class="keywordtype">void</span> unlock() { myMutex.unlock(); }</div><div class="line"></div><div class="line"><span class="keyword">protected</span>:</div><div class="line"></div><div class="line">  <span class="keywordtype">void</span> doTask()</div><div class="line">  {</div><div class="line">    <span class="comment">// print a mark if a joystick button is pressed (other than 1, which is</span></div><div class="line">    <span class="comment">// needed to drive)</span></div><div class="line">    <span class="keywordflow">if</span>(myJoyHandler)</div><div class="line">    {</div><div class="line">      <span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> b = 2; b &lt;= myJoyHandler-&gt;getNumButtons(); ++b)</div><div class="line">        <span class="keywordflow">if</span>(myJoyHandler-&gt;getButton(b)) {</div><div class="line">          <span class="keywordflow">if</span>(!myButtonDown)</div><div class="line">            printf(<span class="stringliteral">&quot;--------------- Joystick button %d pressed.\n&quot;</span>, b);</div><div class="line">          myButtonDown = <span class="keyword">true</span>;</div><div class="line">        }</div><div class="line">        <span class="keywordflow">else</span></div><div class="line">          myButtonDown = <span class="keyword">false</span>;</div><div class="line">    }</div><div class="line"></div><div class="line">    lock();</div><div class="line">    <span class="keywordtype">int</span> f = myGPS-&gt;read(50);</div><div class="line">    printf(<span class="stringliteral">&quot;%.4f\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f&quot;</span></div><div class="line">           <span class="stringliteral">&quot;\t%2.8f\t%2.8f\t%4.4f\t%4.4f&quot;</span></div><div class="line">           <span class="stringliteral">&quot;\t%lu\t%lu\t%s&quot;</span></div><div class="line">           <span class="stringliteral">&quot;\t%u\t%2.4f\t%2.4f\t%2.4f&quot;</span></div><div class="line">           <span class="stringliteral">&quot;\t%s\n&quot;</span>,</div><div class="line">      myRobot-&gt;getX(), myRobot-&gt;getY(), myRobot-&gt;getTh(), myRobot-&gt;getVel(), myRobot-&gt;getRotVel(), (myRobot-&gt;hasLatVel())?(myRobot-&gt;getLatVel()):0,</div><div class="line">      myGPS-&gt;getLatitude(), myGPS-&gt;getLongitude(), myGPS-&gt;getAltitude(), myGPS-&gt;getSpeed(),</div><div class="line">      myGPS-&gt;getGPSPositionTimestamp().getSec(), myGPS-&gt;getGPSPositionTimestamp().getMSec(), myGPS-&gt;getFixTypeName(),</div><div class="line">      myGPS-&gt;getNumSatellitesTracked(), myGPS-&gt;getPDOP(), myGPS-&gt;getHDOP(), myGPS-&gt;getVDOP(),</div><div class="line">      ((f&amp;ArGPS::ReadUpdated)?<span class="stringliteral">&quot;yes&quot;</span>:<span class="stringliteral">&quot;no&quot;</span>)</div><div class="line">    );</div><div class="line">    unlock();</div><div class="line">  }</div><div class="line"></div><div class="line"><span class="keyword">private</span>:</div><div class="line">  <a class="code" href="classArRobot.html">ArRobot</a> *myRobot;</div><div class="line">  <a class="code" href="classArGPS.html">ArGPS</a> *myGPS;</div><div class="line">  <a name="_a4"></a><a class="code" href="classArFunctorC.html">ArFunctorC&lt;GPSLogTask&gt;</a> myTaskFunctor;</div><div class="line">  <a name="_a5"></a><a class="code" href="classArMutex.html">ArMutex</a> myMutex;</div><div class="line">  <a class="code" href="classArJoyHandler.html">ArJoyHandler</a> *myJoyHandler;</div><div class="line">  <span class="keywordtype">bool</span> myButtonDown;</div><div class="line">};</div><div class="line"></div><div class="line"></div><div class="line"></div><div class="line"><span class="keywordtype">int</span> main(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span>** argv)</div><div class="line">{</div><div class="line">  <a name="a6"></a><a class="code" href="classAria.html#ad99c16b5d947229d9f8e1c5b2d4cdd73">Aria::init</a>();</div><div class="line">  <a name="_a7"></a><a class="code" href="classArArgumentParser.html">ArArgumentParser</a> parser(&amp;argc, argv);</div><div class="line">  parser.<a name="a8"></a><a class="code" href="classArArgumentParser.html#a501694b62a7f8b0c3cadd84aeafe1b35">loadDefaultArguments</a>();</div><div class="line">  <a class="code" href="classArRobot.html">ArRobot</a> robot;</div><div class="line">  <a name="_a9"></a><a class="code" href="classArRobotConnector.html">ArRobotConnector</a> robotConnector(&amp;parser, &amp;robot);</div><div class="line">  <a name="_a10"></a><a class="code" href="classArGPSConnector.html">ArGPSConnector</a> gpsConnector(&amp;parser);</div><div class="line">  <a name="_a11"></a><a class="code" href="classArLaserConnector.html">ArLaserConnector</a> laserConnector(&amp;parser, &amp;robot, &amp;robotConnector);</div><div class="line"></div><div class="line">  <span class="comment">// Connect to the robot, get some initial data from it such as type and name,</span></div><div class="line">  <span class="comment">// and then load parameter files for this robot.</span></div><div class="line">  <span class="keywordflow">if</span>(!robotConnector.<a name="a12"></a><a class="code" href="classArRobotConnector.html#ab192ac393d9cc1e5b61b1a82b08d085d">connectRobot</a>())</div><div class="line">  {</div><div class="line">    <a name="a13"></a><a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a name="a14"></a><a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a012daf6573594f91242f8dd7c02eb74b">ArLog::Terse</a>, <span class="stringliteral">&quot;gpsRobotTaskExample: Could not connect to the robot.&quot;</span>);</div><div class="line">    <span class="keywordflow">if</span>(parser.<a name="a15"></a><a class="code" href="classArArgumentParser.html#ac3889cbdfa862031c51bf80e06954914">checkHelpAndWarnUnparsed</a>())</div><div class="line">    {</div><div class="line">        <span class="comment">// -help not given</span></div><div class="line">        <a name="a16"></a><a class="code" href="classAria.html#a10d71f3d4d0cf7f38c58a1f749f64a42">Aria::logOptions</a>();</div><div class="line">        <a name="a17"></a><a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(1);</div><div class="line">    }</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordflow">if</span> (!<a name="a18"></a><a class="code" href="classAria.html#a1b090c01d88bb420b8cf8e0384d25ee1">Aria::parseArgs</a>() || !parser.<a class="code" href="classArArgumentParser.html#ac3889cbdfa862031c51bf80e06954914">checkHelpAndWarnUnparsed</a>())</div><div class="line">  {</div><div class="line">    <a class="code" href="classAria.html#a10d71f3d4d0cf7f38c58a1f749f64a42">Aria::logOptions</a>();</div><div class="line">    <a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(1);</div><div class="line">  }</div><div class="line"></div><div class="line">  <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a name="a19"></a><a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;gpsRobotTaskExample: Connected to robot.&quot;</span>);</div><div class="line"></div><div class="line">  <span class="comment">// Start the robot  running</span></div><div class="line">  robot.<a name="a20"></a><a class="code" href="classArRobot.html#a3c3d878cfd11a2ef91230005a83bfcaa">runAsync</a>(<span class="keyword">true</span>);</div><div class="line"></div><div class="line">  <span class="comment">// Connect to laser(s) as defined in parameter files.</span></div><div class="line">  <span class="keywordflow">if</span>(!laserConnector.<a name="a21"></a><a class="code" href="classArLaserConnector.html#ab97ce6a5fedebf44526773bc674b970e">connectLasers</a>())</div><div class="line">  {</div><div class="line">    <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a012daf6573594f91242f8dd7c02eb74b">ArLog::Terse</a>, <span class="stringliteral">&quot;Warning, Could not connect to configured lasers. &quot;</span>);</div><div class="line">  }</div><div class="line">  </div><div class="line"></div><div class="line">  <a name="_a22"></a><a class="code" href="classArActionLimiterForwards.html">ArActionLimiterForwards</a> nearLimitAction(<span class="stringliteral">&quot;limit near&quot;</span>, 300, 600, 250);</div><div class="line">  <a class="code" href="classArActionLimiterForwards.html">ArActionLimiterForwards</a> farLimitAction(<span class="stringliteral">&quot;limit far&quot;</span>, 300, 1100, 400);</div><div class="line">  <a name="_a23"></a><a class="code" href="classArActionLimiterBackwards.html">ArActionLimiterBackwards</a> limitBackwardsAction;</div><div class="line">  <a name="_a24"></a><a class="code" href="classArActionJoydrive.html">ArActionJoydrive</a> joydriveAction;</div><div class="line">  <a name="_a25"></a><a class="code" href="classArActionKeydrive.html">ArActionKeydrive</a> keydriveAction;</div><div class="line"></div><div class="line"></div><div class="line">  <span class="comment">// Connect to GPS</span></div><div class="line">  <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;gpsRobotTaskExample: Connecting to GPS, it may take a few seconds...&quot;</span>);</div><div class="line">  <a class="code" href="classArGPS.html">ArGPS</a> *gps = gpsConnector.<a name="a26"></a><a class="code" href="classArGPSConnector.html#a4a58ea7f81bc75b8ddeea820ac541c48">createGPS</a>(&amp;robot);</div><div class="line">  assert(gps);</div><div class="line">  <span class="keywordflow">if</span>(!gps || !(gps-&gt;<a name="a27"></a><a class="code" href="classArGPS.html#a5edda03cc9e4c8db380e18f8d5465b7e">connect</a>()))</div><div class="line">  {</div><div class="line">    <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a012daf6573594f91242f8dd7c02eb74b">ArLog::Terse</a>, <span class="stringliteral">&quot;gpsRobotTaskExample: Error connecting to GPS device.  Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help. Exiting.&quot;</span>);</div><div class="line">    <span class="keywordflow">return</span> -3;</div><div class="line">  }</div><div class="line"></div><div class="line"></div><div class="line">  <span class="comment">// Create an GPSLogTask which will register a task with the robot.</span></div><div class="line">  GPSLogTask gpsTask(&amp;robot, gps, joydriveAction.<a name="a28"></a><a class="code" href="classArActionJoydrive.html#a9bfb87ceded10e06d0b34052ca692711">getJoyHandler</a>()-&gt;<a name="a29"></a><a class="code" href="classArJoyHandler.html#a03a02f2ff48626658e8323436a324814">haveJoystick</a>() ? joydriveAction.<a class="code" href="classArActionJoydrive.html#a9bfb87ceded10e06d0b34052ca692711">getJoyHandler</a>() : NULL);</div><div class="line"></div><div class="line"></div><div class="line">  <span class="comment">// Add actions</span></div><div class="line">  robot.<a name="a30"></a><a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;nearLimitAction, 100);</div><div class="line">  robot.<a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;farLimitAction, 90);</div><div class="line">  robot.<a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;limitBackwardsAction, 80);</div><div class="line">  robot.<a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;joydriveAction, 50);</div><div class="line">  robot.<a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;keydriveAction, 40);</div><div class="line"></div><div class="line">  <span class="comment">// allow keydrive action to drive robot even if joystick button isn&#39;t pressed</span></div><div class="line">  joydriveAction.<a name="a31"></a><a class="code" href="classArActionJoydrive.html#a2d2e8053a6a49487c5dbeadcbcc44556">setStopIfNoButtonPressed</a>(<span class="keyword">false</span>);</div><div class="line"></div><div class="line"></div><div class="line">  robot.<a name="a32"></a><a class="code" href="classArRobot.html#a5aecc7ac998c9a31b82fdb904efa8609">lock</a>();</div><div class="line"></div><div class="line">  robot.<a name="a33"></a><a class="code" href="group__easy.html#gaa3657bebab0086dd14ed0c8c3f76dc8b">enableMotors</a>();</div><div class="line">  robot.<a name="a34"></a><a class="code" href="classArRobot.html#aceef80a6274fb34aafcc422e4b1778c1">comInt</a>(47, 1);  <span class="comment">// enable joystick driving on some robots</span></div><div class="line"></div><div class="line">  <span class="comment">// Add exit callback to reset/unwrap steering wheels on seekur (critical if the robot doesn&#39;t have sliprings); does nothing for other robots </span></div><div class="line">  <a name="a35"></a><a class="code" href="classAria.html#a5c4bfae6980dd3a7655a4cc7b32d1aa4">Aria::addExitCallback</a>(<span class="keyword">new</span> <a name="_a36"></a><a class="code" href="classArRetFunctor1C.html">ArRetFunctor1C&lt;bool, ArRobot, unsigned char&gt;</a>(&amp;robot, &amp;<a name="a37"></a><a class="code" href="classArRobot.html#a165e27bc81ed9f159820dfdf215d5500">ArRobot::com</a>, (<span class="keywordtype">unsigned</span> <span class="keywordtype">char</span>)120));</div><div class="line">  <a class="code" href="classAria.html#a5c4bfae6980dd3a7655a4cc7b32d1aa4">Aria::addExitCallback</a>(<span class="keyword">new</span> <a class="code" href="classArRetFunctor1C.html">ArRetFunctor1C&lt;bool, ArRobot, unsigned char&gt;</a>(&amp;robot, &amp;<a class="code" href="classArRobot.html#a165e27bc81ed9f159820dfdf215d5500">ArRobot::com</a>, (<span class="keywordtype">unsigned</span> <span class="keywordtype">char</span>)120));</div><div class="line"></div><div class="line">  robot.<a name="a38"></a><a class="code" href="classArRobot.html#a403d9de010dc2b8d478cd0b14b2d9491">unlock</a>();</div><div class="line"></div><div class="line">  <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;gpsRobotTaskExample: Running... (drive robot with joystick or arrow keys)&quot;</span>);</div><div class="line">  robot.<a name="a39"></a><a class="code" href="group__easy.html#ga95201be7c39b521983b51e6f5d9303e4">waitForRunExit</a>();</div><div class="line"></div><div class="line"></div><div class="line">  <span class="keywordflow">return</span> 0;</div><div class="line">}</div></div><!-- fragment --> </div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
  </ul>
</div>
</body>
</html>
